//
// = FILENAME
//    Serializer.hh
//
// = AUTHOR(S)
//    Patric Jensfelt
//
// = COPYRIGHT
//    Copyright (c) 2010 Patric Jensfelt
//
/*----------------------------------------------------------------------*/

#ifndef Serializer_hh
#define Serializer_hh

#include <termios.h>
#include <sys/time.h>
#include <string>
#include <vector>

/**
 * This class provides an interface to the Serializer board from
 * RoboticsConnection. It uses a simple blocking communication
 * interface.
 *
 * @author Patric Jensfelt
 * @see
 */
class Serializer {
public:
  enum EncoderType {
    ENCODER_SINGLE = 0,
    ENCODER_QUAD
  };

  enum UnitType {
    UNIT_METRIC = 0,
    UNIT_ENGLISH,
    UNIT_RAW
  };

public:
  /**
   * Constructor
   */
  Serializer();

  /**
   * Destructor that will call disconnect()
   */
  ~Serializer();

  /**
   * Use this function to connect to the Serializer
   * @return true if connection ok, else false
   */
  bool connect(const std::string &device = "/dev/ttyS2", int baudrate = 115200);

  /**
   * Use thsi function to disconnect from the Serialiser. This
   * function will be automatically called from the destructor for
   * Serializer.
   */
  void disconnect();

  /**
   * Use this function to restore the board to the factory
   * settings. This means for example that the baudrate will be 19200.
   * Will not return anything useful since connection will be lost
   * unless communication was running at 19200 which is unlikely.
   * @return true always if connected.
   */
  bool restoreToFactoryDefaults();

  /**
   * This function will send a reset to the board so that it
   * reboots. It will take a few seconds before the board is back up
   * again so this function will block for a while.
   * @return true if command was successful
   */
  bool resetBoard();

  /**
   * Use this function to change the baudrate for the Serializer. It
   * requires that you have established a connection first. After
   * issuing this command you need to disconnest and connect again at
   * the new baudrate. This baudrate will be stored in the EEPROM on
   * the Serializer and will therefore be the baudrate to use until
   * someone changes the baudrate again or restores the settings to
   * the factory defaults.
   * @param baudrate desired baudrate 2400,4800,9600,19200,57600,115200
   * @return true if command was ACKnowledged
   */
  bool setBaudrate(int baudrate);

  /**
   * Use this function to set what unit to use. Supported are Metric,
   * English and Raw.
   * @param unit
   * @return true if command was ACKnowledged
   */
  bool setUnit(enum UnitType u);

  /**
   * This function sets the frequency in the blinking for the LED on
   * the Serializer board.
   * @param freq interger 1(slow) - 127(fast)
   * @return true if command was ACKnowledged
   */
  bool setLEDFreq(int freq);

  /**
   * This function allows you to specify what type of encoder we have.
   * @param type can be either ENCODER_SINGLE and ENCODER_QUAD
   * @return true if command was ACKnowledged 
   */
  bool setEncoderType(enum EncoderType e);

  /**
   * Get the count from one of the encoders
   * @param channel 0 for the first chanel and 1 for the second.
   * @param enc encoder count
   * @return true if encoder data was returned
   */
  bool getEncoder(int channel, long &enc);

  /**
   * Get the count from both encoders
   * @param enc0 encounter count for channel 0
   * @param enc1 encounter count for channel 1
   * @return true if encoder data was returned
   */
  bool getEncoders(long &enc0, long &enc1);

  /**
   * Clear the encoder counter for one of the encoder channels
   * @return true if command was ACKnowledged 
   */
  bool clearEncoder(int channel);

  /**
   * Clear both encoder counters
   * @return true if command was ACKnowledged 
   */
  bool clearEncoders();

  /**
   * Get the PIDL parameters for the VPID controller 
   * (see Serializer manual for details)
   * @param P proportional gain
   * @param I gain for integral part of controller
   * @param D gain for derivative part of the controller
   * @param L loop multiplier (multiples of 1.6ms to accumulate encoder data)
   * @return true if command was ACKnowledged 
   */
  bool getVPID(int &P, int &I, int &D, int &L);  

  /**
   * Set the PIDL parameters for the VPID controller 
   * (see Serializer manual for details)
   * @param P proportional gain
   * @param I gain for integral part of controller
   * @param D gain for derivative part of the controller
   * @param L loop multiplier (multiples of 1.6ms to accumulate encoder data)
   * @return true if command was ACKnowledged 
   */
  bool setVPID(int P, int I, int D, int L);

  /**
   * Get the PIDA parameters for the DPID controller 
   * (see Serializer manual for details)
   * @param P proportional gain
   * @param I gain for integral part of controller
   * @param D gain for derivative part of the controller
   * @param A acceleration
   * @return true if command was ACKnowledged 
   */
  bool getDPID(int &P, int &I, int &D, int &A);  

  /**
   * Set the PIDA parameters for the DPID controller 
   * (see Serializer manual for details)
   * @param P proportional gain
   * @param I gain for integral part of controller
   * @param D gain for derivative part of the controller
   * @param A acceleration
   * @return true if command was ACKnowledged 
   */
  bool setDPID(int P, int I, int D, int A);

  /**
   * Set PWM value for one of the motors
   * @param motor the motor to control 0 or 1
   * @param pwm the desired PWM value for the motor
   * @return true if command was ACKnowledged 
   */
  bool setMotorPWM(int motor, int pwm);

  /**
   * Set PWM value for the motors
   * @param pwm0 the desired PWM for motor 0
   * @param pwm1 the desired PWM for motor 1
   * @return true if command was ACKnowledged 
   */
  bool setMotorPWMs(int pwm0, int pwm1);

  /**
   * Set motor speed
   * @param motor the motor to control 0 or 1
   * @param speed the desired speed (depends on VPID parameters)
   * @return true if command was ACKnowledged 
   */
  bool setMotorSpeed(int motor, int speed);

  /**
   * Set motor speed
   * @param speed0 the desired speed for motor 0 (depends on VPID parameters)
   * @param speed1 the desired speed for motor 1 (depends on VPID parameters)
   * @return true if command was ACKnowledged 
   */
  bool setMotorSpeeds(int speed0, int speed1);

  /**
   * This function alows you to ask for the speed on the two motors
   * @param speed0 the speed of motor 0
   * @param speed1 the speed of motor 1
   * @return true if data was received and could be parsed
   */
  bool getMotorSpeeds(int &speed0, int &speed1);

  /**
   * Use this function to check if a DIGO function is busy still or
   * not (this wraps the function "pids" from the Serializer).
   * @param busy will be set to the result of the query
   * @return true if command was ACKnowledged 
   */
  bool getDIGOstate(bool &busy);

  /**
   * This function stops all motor motion immediately
   * @return true if command was ACKnowledged
   */
  bool stopAllMotors();

  /**
   * Read A/D channels
   * @param query vector with int-pairs where the first entry contain
   * the id of the A/D channel to poll and the second will contain the
   * reading after the call
   * @return true if data was received
   * 
   * @code
   * //To query channels 0 and 5 do
   * std::vector< std::pair<int,int> > query;
   * cmd.resize(2);
   * query[0].first = 0;
   * query[1].first = 5;
   * readADC(query); 
   * std::cerr << "c0=" << query[0].second << " c1=" << query[1].second
   *           << std::endl;
   * @endcode
   */
  bool readADC(std::vector< std::pair<int, int> > &query);

  /**
   * Set GPIO pins
   * @param cmd vector with int-bool-pairs where the first entry contain
   * the id of the pin to set and the second the value to set it to
   * @return true if command was ACKnowledged
   * 
   * @code
   * //To set pin 0 to false and pin 3 to true do
   * std::vector< std::pair<int,bool> > cmd;
   * query.push_back(std::make_pair(0,false));
   * query.push_back(std::make_pair(3,true));
   * setIO(cmd); 
   * @endcode
   */
  bool setIO(std::vector< std::pair<int, bool> > &cmd);

  /**
   * Get status of GPIO pins
   * @param query vector with int-bool-pairs where the first entry contain
   * the id of the pin to set and the second the value to of that input IO.
   * @return true if data was received
   * 
   * @code
   * //To get status of pin 0 and pin 3 do
   * std::vector< std::pair<int,bool> > cmd;
   * cmd.resize(2);
   * query[0].first = 0;
   * query[1].first = 3;
   * getIO(cmd); 
   * std::cerr << "pin" << query[0].first << "=" << query[0].second
   *           << query[1].first << "=" << query[1].second
   *           << std::endl;
   * @endcode
   */
  bool getIO(std::vector< std::pair<int, bool> > &cmd);

  /**
   * This function reads data from a SRF08 sonar. The value that you
   * get depends on the setting for the units. 
   * @param value the value read (output)
   * @param address the address on the I2C buf (default 0xE0)
   */
  bool readSRF08(int &value, int address = 0xE0);

  /**
   * This function allows you to set the position of a number of
   * servos. You specifies pairs with servo id and position. The id
   * corresponds to the port it is connected to which can be 4-9. If
   * you specify a port outside this interval you will get an error
   * and no command will be sent.
   *
   * @cmd command vector with int-pairs where the first entry contain
   * the servo post (4-9) and the second will contain the desired position
   * @return true if command was ACKnowledged
   * 
   * @code
   * //To set servo on port 4 to pos 42 and servo on port 8 to pos -22 do
   * std::vector< std::pair<int,int> > cmd;
   * cmd.push_back(std::make_pair(4,42));
   * cmd.push_back(std::make_pair(8,-22));
   * setServoPos(cmd); 
   * @endcode
   */
  bool setServoPos(std::vector< std::pair<int, int> > &cmd);

  static inline double getCurrentTime()
  {
    struct timeval tv;
    gettimeofday(&tv, NULL);
    return (1.0*tv.tv_sec + 1e-6*tv.tv_usec);
  }

private:
  /**
   * Empties the recv buffer
   */
  void emptyRecvBuf() { m_RecvBufPos = 0; }

  /**
   * Set the terminal config
   * @return 0 if OK
   */
  int setTermios(int baudrate);

  /**
   * Waits for data on the serial port for at most maxSecs seconds.
   * @return 1 got data, 0 if timeout, -1 for error
   */
  int waitSerialBuffer(double maxSecs);

  /**
   * Read from serial port into recvBuf
   * @return number of read bytes
   */
  int readToRecvBuf();

  /**
   * Wait for a reply from the Serializer. The function waits until
   * the trailing CR comes. It waits at most maxSecs seconds.
   * @return 0 if OK, else <0
   */
  int blockingRecvReply(double maxSecs);

  /** 
   * Waits at most maxSeconds for an ACK.
   * @return 0 if we got the ACK in maxSecs
   */
  int waitForACK(double maxSecs);

  /**
   * This function sends a message and waits for the ACK. If we get
   * the ACK we assume that the connection is OK.
   */
  int handshake();

  /**
   * This function send a message to the device. It will use strlen to
   * tell the length of the string. This means that you need to make
   * sure that the string you want to send is terminated by a \0.
   * @return 0 if sending OK
   */
  int sendMsg(const char *msg);

  /**
   * The id of the servo pins are not the same as the port number
   * written next to them. This function provides the mapping from the
   * number written on the port and the pinid that you need to send to
   * the Serializer. 
   */
  int getServoPinIdFromPortNum(int num);

  /**
   * The id of the servo pins are not the same as the port number
   * written next to them. This function provides the mapping from the
   * pinid that the Serializer talks about and the number written at
   * the port.
   */
  int getServoPortNumFromPinId(int id);

private:
  int m_Fd;

  char *m_RecvBuf;
  int m_RecvBufSize;
  int m_RecvBufPos;

  double m_ReplyTimeout;

  struct termios m_OrigTermios;
};

#endif // class Serializer_hh
